/*
	Functions for turning motors on and off
	
	author: Jared Bayne (JaredNBayne@gmail.com)



*/



int PrevDir=Forward;
int m1PrevSpd=0;
int m2PrevSpd=0;
double m1Skip=0;
double m1Skip2=0;
double m2Skip=0;
double m2Skip2=0;


#define Acceleration_Value 1
#define Addition_Skipper 50


#define		left	0
#define		right	1

int M1Last=0;
int M2Last=0;
int M1LastDir=Forward;
int M2LastDir=Forward;

// m = motor
void go(int m1dir, int m1spd, int m2dir, int m2spd) // speed switch function
{
	M1Last=m1spd;
	M2Last=m2spd;
	M1LastDir=m1dir;
	M2LastDir=m2dir;
	switch (m1dir)	//Motor One
	{
		case 1:		//Forward
			M1C2 = 255-m1spd;	//Set Speed
			M1C1 = ON;		//Set Direction
			break;
		case 2:		//Backward
			M1C2 = m1spd;	//Set Speed
			M1C1 = OFF;		//Set Direction
			break;
		default:	//Bad Direction
			writeln("BAD DIRECTION MOTOR 1");
			
	}
	switch (m2dir)	//Motor One
	{
		case 1:		//Forward
			M2C2 = m2spd;	//Set Speed
			M2C1 = OFF;		//Set Direction
			break;
		case 2:		//Backward
			M2C2 = 255-m2spd;	//Set Speed
			M2C1 = ON;		//Set Direction
			break;
		default:	//Bad Direction
			writeln("BAD DIRECTION MOTOR 2");
			
	}
}

void stop(void)
{
	go(Backward,0,Backward,0);	//Stop Motors

}


void goforward(int speed) //forward motion function
{

	go(Forward,speed,Forward,speed);
}
void goleft(int speed) //Tank turn left
{
	go(Backward,speed,Forward,speed);
}
void goright(int speed) //Tank turn right
{
	go(Forward,speed,Backward,speed);

}
void gobackward(int speed) //backward motion function
{
	go(Backward,speed,Backward,speed);

}

void motors_init()
{
	// set up PB0 and PB1 with 8bit PWM
	TCCR1A = _BV(WGM10)  // 8 BIT, PHASE CORRECT PWM
		   | _BV(COM1A1)
		   | _BV(COM1B1);
	TCCR1B = _BV(CS10)   // NO PRESCALE
		   | _BV(WGM12);  // FAST PWM
		   
}

// what's my range? What's max, what's minimum?
void goleft_perfect(int distance, int speed)
{
/*
	This function will make the robot go left
	and attempt to restrict the natural curve of the robot's movement
	
	Wheel encoders (Enc1 & Enc2) are used to determine the distance
	
	distance should be given in wheel encoder ticks (how many ticks per rotation)
*/
	#define diff_offset 0	//allowable difference in speed
	#define	correction	1	//how much to correct each time
	#define max_difference 75	//maximum difference allowed between two speeds
	#define	min_speed	100
	

	int speed1_corrected = speed, speed2_corrected = speed;

	// Enc stands for Wheel encoder 1, and wheel encoder 2.  (The pinwheel looking things on the inner wheel
	Enc1=Enc2=0;
	while ( (Enc1 < distance) && (Enc2<distance) )	//while both sides haven't gone far enough
	{
		//calculate corrected speeds
		if (Enc1 > (Enc2 + diff_offset))
		{
			speed1_corrected-=correction;	//slow down speed1
			speed2_corrected+=correction;
		}
		else if (Enc1 < (Enc2 - diff_offset))
		{
			speed2_corrected-=correction;
			speed1_corrected+=correction;	//spped up speed1
		}
		else if (Enc1==Enc2)
		{
			speed1_corrected=speed;
			speed2_corrected=speed;
		}
		//take care of max outs based on max_difference
		if (speed1_corrected<(speed-max_difference))
			speed1_corrected=speed;
		else if (speed1_corrected>(speed+max_difference))
			speed1_corrected=speed;
		
		if (speed2_corrected<(speed-max_difference))
			speed2_corrected=speed;
		else if (speed2_corrected>(speed+max_difference))
			speed2_corrected=speed;
				
		
		//take care of max outs based on physical limits of robot
		if (speed1_corrected<min_speed)
			speed1_corrected=min_speed;
		else if (speed1_corrected>254)
			speed1_corrected=254;
		
		if (speed2_corrected<min_speed)
			speed2_corrected=min_speed;
		else if (speed2_corrected>254)
			speed2_corrected=254;
		
		
		go(Backward, speed1_corrected, Forward, speed2_corrected);
	
	}
	


}

void goright_perfect(int distance, int speed)
{
/*
	This function will make the robot go right
	and attempt to restrict the natural curve of the robot's movement
	
	Wheel encoders are used to determine the distance
	
	distance should be given in wheel encoder ticks
*/
	#define diff_offset 0	//allowable difference in speed
	#define	correction	1	//how much to correct each time
	#define max_difference 75	//maximum difference allowed between two speeds
	#define	min_speed	100
	

	int speed1_corrected = speed, speed2_corrected = speed;

	
	Enc1=Enc2=0;
	while ( (Enc1 < distance) && (Enc2<distance) )	//while both sides haven't gone far enough
	{
		//calculate corrected speeds
		if (Enc1 > (Enc2 + diff_offset))
		{
			speed1_corrected-=correction;	//slow down speed1
			speed2_corrected+=correction;
		}
		else if (Enc1 < (Enc2 - diff_offset))
		{
			speed2_corrected-=correction;
			speed1_corrected+=correction;	//spped up speed1
		}
		else if (Enc1==Enc2)
		{
			speed1_corrected=speed;
			speed2_corrected=speed;
		}
		//take care of max outs based on max_difference
		if (speed1_corrected<(speed-max_difference))
			speed1_corrected=speed;
		else if (speed1_corrected>(speed+max_difference))
			speed1_corrected=speed;
		
		if (speed2_corrected<(speed-max_difference))
			speed2_corrected=speed;
		else if (speed2_corrected>(speed+max_difference))
			speed2_corrected=speed;
				
		
		//take care of max outs based on physical limits of robot
		if (speed1_corrected<min_speed)
			speed1_corrected=min_speed;
		else if (speed1_corrected>254)
			speed1_corrected=254;
		
		if (speed2_corrected<min_speed)
			speed2_corrected=min_speed;
		else if (speed2_corrected>254)
			speed2_corrected=254;
		
		
		go(Forward, speed1_corrected, Backward, speed2_corrected);
	
	}


}

void goforward_perfectlystraight_continuous(int speed)
{
/*

	Much the same as goforward_perfectlystraight() except that
	there is no stopping condition.  Basically, this function
	will work like all the other "go" functions (i.e. goforward()).
	
	So, the encoders must be reset prior to the first time
	calling this function in the loop that is calling it.
	
	
	
*/

	int speed1_corrected = speed, speed2_corrected = speed;


	//calculate corrected speeds
	if (Enc1 > (Enc2 + diff_offset))
	{
		speed1_corrected-=correction;	//slow down speed1
		speed2_corrected+=correction;
	}
	else if (Enc1 < (Enc2 - diff_offset))
	{
		speed2_corrected-=correction;
		speed1_corrected+=correction;	//spped up speed1
	}
	else if (Enc1==Enc2)
	{
		speed1_corrected=speed;
		speed2_corrected=speed;
	}
	//take care of max outs based on max_difference
	if (speed1_corrected<(speed-max_difference))
		speed1_corrected=speed;
	else if (speed1_corrected>(speed+max_difference))
		speed1_corrected=speed;
	
	if (speed2_corrected<(speed-max_difference))
		speed2_corrected=speed;
	else if (speed2_corrected>(speed+max_difference))
		speed2_corrected=speed;
			
	
	//take care of max outs based on physical limits of robot
	if (speed1_corrected<min_speed)
		speed1_corrected=min_speed;
	else if (speed1_corrected>254)
		speed1_corrected=254;
	
	if (speed2_corrected<min_speed)
		speed2_corrected=min_speed;
	else if (speed2_corrected>254)
		speed2_corrected=254;
		
		
	go(Forward, speed1_corrected, Forward, speed2_corrected);

}

void goforward_perfectlystraight(int distance, int speed)
{


/*
	This function will make the robot go forward
	and attempt to restrict the natural curve of the robot's movement
	
	Wheel encoders are used to determine the distance
	
	distance should be given in wheel encoder ticks
*/
	#define diff_offset 0	//allowable difference in speed
	#define	correction	1	//how much to correct each time
	#define max_difference 75	//maximum difference allowed between two speeds
	#define	min_speed	100
	

	int speed1_corrected = speed, speed2_corrected = speed;

	
	Enc1=Enc2=0;
	while ( (Enc1 < distance) && (Enc2<distance) )	//while both sides haven't gone far enough
	{
		//calculate corrected speeds
		if (Enc1 > (Enc2 + diff_offset))
		{
			speed1_corrected-=correction;	//slow down speed1
			speed2_corrected+=correction;
		}
		else if (Enc1 < (Enc2 - diff_offset))
		{
			speed2_corrected-=correction;
			speed1_corrected+=correction;	//spped up speed1
		}
		else if (Enc1==Enc2)
		{
			speed1_corrected=speed;
			speed2_corrected=speed;
		}
		//take care of max outs based on max_difference
		if (speed1_corrected<(speed-max_difference))
			speed1_corrected=speed;
		else if (speed1_corrected>(speed+max_difference))
			speed1_corrected=speed;
		
		if (speed2_corrected<(speed-max_difference))
			speed2_corrected=speed;
		else if (speed2_corrected>(speed+max_difference))
			speed2_corrected=speed;
				
		
		//take care of max outs based on physical limits of robot
		if (speed1_corrected<min_speed)
			speed1_corrected=min_speed;
		else if (speed1_corrected>254)
			speed1_corrected=254;
		
		if (speed2_corrected<min_speed)
			speed2_corrected=min_speed;
		else if (speed2_corrected>254)
			speed2_corrected=254;
		
		
		go(Forward, speed1_corrected, Forward, speed2_corrected);
	
	}
	
}

void gobackward_perfectlystraight(int distance, int speed)
{
/*
	This function will make the robot go backward
	and attempt to restrict the natural curve of the robot's movement
	
	Wheel encoders are used to determine the distance
	
	distance should be given in wheel encoder ticks
*/
	#define diff_offset 0	//allowable difference in speed
	#define	correction	1	//how much to correct each time
	#define max_difference 75	//maximum difference allowed between two speeds

	

	int speed1_corrected = speed, speed2_corrected = speed;

	
	Enc1=Enc2=0;
	while ( (Enc1 < distance) && (Enc2<distance) )	//while both sides haven't gone far enough
	{
		//calculate corrected speeds
		if (Enc1 > (Enc2 + diff_offset))
		{
			speed1_corrected-=correction;	//slow down speed1
			speed2_corrected+=correction;
		}
		else if (Enc1 < (Enc2 - diff_offset))
		{
			speed2_corrected-=correction;
			speed1_corrected+=correction;	//spped up speed1
		}
		else if (Enc1==Enc2)
		{
			speed1_corrected=speed;
			speed2_corrected=speed;
		}
		//take care of max outs
		if (speed1_corrected<(speed-max_difference))
			speed1_corrected=speed;
		else if (speed1_corrected>(speed+max_difference))
			speed1_corrected=speed;
		
		if (speed2_corrected<(speed-max_difference))
			speed2_corrected=speed;
		else if (speed2_corrected>(speed+max_difference))
			speed2_corrected=speed;
				
		//take care of max outs based on physical limits of robot
		if (speed1_corrected<min_speed)
			speed1_corrected=min_speed;
		else if (speed1_corrected>254)
			speed1_corrected=254;
		
		if (speed2_corrected<min_speed)
			speed2_corrected=min_speed;
		else if (speed2_corrected>254)
			speed2_corrected=254;
		
		
		go(Backward, speed1_corrected, Backward, speed2_corrected);
	
	}
}
